
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mc_circle.c
  * @author     baiyang
  * @date       2022-10-10
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mc_circle.h"

#include <parameter/param.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void circlenav_init_start_angle(mc_circle_t circle, bool use_heading);
static void circlenav_calc_velocities(mc_circle_t circle, bool init_velocity);
static enum CircleTerrainSource circlenav_get_terrain_source(mc_circle_t circle);
static bool circlenav_get_terrain_offset(mc_circle_t circle, float* offset_cm);
static void assign_param(mc_circle_t circle);
/*----------------------------------variable----------------------------------*/
static param_t var_info[] = {
    PARAM_DEFINE_FLOAT(CIRCLE_RADIUS,   MB_CIRCLE_RADIUS_DEFAULT),
    PARAM_DEFINE_FLOAT(CIRCLE_RATE,     MB_CIRCLE_RATE_DEFAULT),
    PARAM_DEFINE_INT16(CIRCLE_OPTIONS,  1),
};
PARAM_GROUP_DEFINE(CIRCLE, var_info);
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/// Constructor
void circlenav_ctor(mc_circle_t circle, const ahrs_view* ahrs, Position_ctrl* pos_control)
{
    assign_param(circle);

    circle->_ahrs = ahrs;
    circle->_pos_control = pos_control;
    circle->_yaw = 0.0f;
    circle->_angle = 0.0f;
    circle->_angle_total = 0.0f;
    circle->_angular_vel = 0.0f;
    circle->_angular_vel_max = 0.0f;
    circle->_angular_accel = 0.0f;

    // init flags
    circle->_flags.panorama = false;
}

/// init - initialise circle controller setting center specifically
///     set terrain_alt to true if center.z should be interpreted as an alt-above-terrain
///     caller should set the position controller's x,y and z speeds and accelerations before calling this
void circlenav_init(mc_circle_t circle, const Vector3p* center, bool terrain_alt)
{
    circle->_center = *center;
    circle->_terrain_alt = terrain_alt;
    // initialise position controller (sets target roll angle, pitch angle and I terms based on vehicle current lean angles)
    posctrl_init_xy_controller_stopping_point(circle->_pos_control);
    posctrl_init_z_controller_stopping_point(circle->_pos_control);

    // calculate velocities
    circlenav_calc_velocities(circle, true);

    // set start angle from position
    circlenav_init_start_angle(circle, false);
}

/// init - initialise circle controller setting center using stopping point and projecting out based on the copter's heading
///     caller should set the position controller's x,y and z speeds and accelerations before calling this
void circlenav_init2(mc_circle_t circle)
{
    // initialize radius from params
    circle->_radius = circle->_radius_parm;
    circle->_last_radius_param = circle->_radius_parm;

    // initialise position controller (sets target roll angle, pitch angle and I terms based on vehicle current lean angles)
    posctrl_init_xy_controller_stopping_point(circle->_pos_control);
    posctrl_init_z_controller_stopping_point(circle->_pos_control);

    // get stopping point
    const Vector3p stopping_point = posctrl_get_pos_target_cm(circle->_pos_control);

    // set circle center to circle_radius ahead of stopping point
    circle->_center = stopping_point;
    if ((circle->_options & CIRCLE_OPTIONS_INIT_AT_CENTER) == 0) {
        circle->_center.x += circle->_radius * circle->_ahrs->cos_yaw;
        circle->_center.y += circle->_radius * circle->_ahrs->sin_yaw;
    }
    circle->_terrain_alt = false;

    // calculate velocities
    circlenav_calc_velocities(circle, true);

    // set starting angle from vehicle heading
    circlenav_init_start_angle(circle, true);
}

/// set circle center to a Location
void circlenav_set_center(mc_circle_t circle, const Location* center)
{
    if (location_get_alt_frame(center) == ALT_FRAME_ABOVE_TERRAIN) {
        // convert Location with terrain altitude
        Vector2f_t center_xy;
        int32_t terr_alt_cm;
        if (location_get_vector_xy_from_origin_ne(center, &center_xy) && location_get_alt_cm(center, ALT_FRAME_ABOVE_TERRAIN, &terr_alt_cm)) {
            Vector3f_t center_xyz = {center_xy.x, center_xy.y, terr_alt_cm};
            circlenav_set_center2(circle, &center_xyz, true);
        } else {
            Vector3f_t center_xyz = ahrs_get_position_neu_cm(circle->_ahrs);

            // failed to convert location so set to current position and log error
            circlenav_set_center2(circle, &center_xyz, false);
            //AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_CIRCLE_INIT);
        }
    } else {
        // convert Location with alt-above-home, alt-above-origin or absolute alt
        Vector3f_t circle_center_neu;
        if (!location_get_vector_from_origin_neu(center, &circle_center_neu)) {
            // default to current position and log error
            circle_center_neu = ahrs_get_position_neu_cm(circle->_ahrs);
            //AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_CIRCLE_INIT);
        }
        circlenav_set_center2(circle, &circle_center_neu, false);
    }
}

/// set_circle_rate - set circle rate in degrees per second
void circlenav_set_rate(mc_circle_t circle, float deg_per_sec)
{
    if (!math_flt_equal(deg_per_sec, circle->_rate)) {
        param_set_obj(PARAM_ID(CIRCLE, CIRCLE_RATE), deg_per_sec);
    }
}

/// set_circle_rate - set circle rate in degrees per second
void circlenav_set_radius_cm(mc_circle_t circle, float radius_cm)
{
    circle->_radius = math_constrain_float(radius_cm, 0, MB_CIRCLE_RADIUS_MAX);
}

/// returns true if update has been run recently
/// used by vehicle code to determine if get_yaw() is valid
bool circlenav_is_active(mc_circle_t circle)
{
    return (time_millis() - circle->_last_update_ms < 200);
}

/// update - update circle controller
bool circlenav_update(mc_circle_t circle, float climb_rate_cms)
{
    circlenav_calc_velocities(circle, false);

    // calculate dt
    const float dt = posctrl_get_dt(circle->_pos_control);

    // ramp angular velocity to maximum
    if (circle->_angular_vel < circle->_angular_vel_max) {
        circle->_angular_vel += fabsf(circle->_angular_accel) * dt;
        circle->_angular_vel = MIN(circle->_angular_vel, circle->_angular_vel_max);
    }
    if (circle->_angular_vel > circle->_angular_vel_max) {
        circle->_angular_vel -= fabsf(circle->_angular_accel) * dt;
        circle->_angular_vel = MAX(circle->_angular_vel, circle->_angular_vel_max);
    }

    // update the target angle and total angle traveled
    float angle_change = circle->_angular_vel * dt;
    circle->_angle += angle_change;
    circle->_angle = math_wrap_PI(circle->_angle);
    circle->_angle_total += angle_change;

    // calculate terrain adjustments
    float terr_offset = 0.0f;
    if (circle->_terrain_alt && !circlenav_get_terrain_offset(circle, &terr_offset)) {
        return false;
    }

    // calculate z-axis target
    float target_z_cm;
    if (circle->_terrain_alt) {
        target_z_cm = circle->_center.z + terr_offset;
    } else {
        target_z_cm = posctrl_get_pos_target_z_cm(circle->_pos_control);
    }

    // if the circle_radius is zero we are doing panorama so no need to update loiter target
    Vector3p target = {
        circle->_center.x,
        circle->_center.y,
        target_z_cm
    };
    if (!math_flt_zero(circle->_radius)) {
        // calculate target position
        target.x += circle->_radius * cosf(-circle->_angle);
        target.y += - circle->_radius * sinf(-circle->_angle);

        // heading is from vehicle to center of circle
        circle->_yaw = vec2_get_bearing_cd((const Vector2f_t *)&circle->_ahrs->relpos_cm, (const Vector2f_t *)&circle->_center);

        if ((circle->_options & CIRCLE_OPTIONS_FACE_DIRECTION_OF_TRAVEL) != 0) {
            circle->_yaw += math_flt_positive(circle->_rate)?-9000.0f:9000.0f;
            circle->_yaw = math_wrap_360_cd(circle->_yaw);
        }

    } else {
        // heading is same as _angle but converted to centi-degrees
        circle->_yaw = circle->_angle * DEGX100;
    }

    // update position controller target
    Vector2f_t zero = VECTOR2F_DEFAULT_VALUE;
    posctrl_input_pos_vel_accel_xy(circle->_pos_control, (Vector2p*)&target, &zero, &zero, true);
    if (circle->_terrain_alt) {
        float zero2 = 0;
        float target_zf = target.z;
        posctrl_input_pos_vel_accel_z(circle->_pos_control, &target_zf, &zero2, 0, true);
    } else {
        posctrl_set_pos_target_z_from_climb_rate_cm(circle->_pos_control, climb_rate_cms);
    }

    // update position controller
    posctrl_update_xy_controller(circle->_pos_control);

    // set update time
    circle->_last_update_ms = time_millis();

    return true;
}

// get_closest_point_on_circle - returns closest point on the circle
//  circle's center should already have been set
//  closest point on the circle will be placed in result
//  result's altitude (i.e. z) will be set to the circle_center's altitude
//  if vehicle is at the center of the circle, the edge directly behind vehicle will be returned
void circlenav_get_closest_point_on_circle(mc_circle_t circle, Vector3f_t *result)
{
    // return center if radius is zero
    if (circle->_radius <= 0) {
        *result = circle->_center;
        return;
    }

    // get current position
    Vector2p stopping_point;
    posctrl_get_stopping_point_xy_cm(circle->_pos_control, &stopping_point);

    // calc vector from stopping point to circle center
    Vector2f_t vec = {stopping_point.x - circle->_center.x, stopping_point.y - circle->_center.y};
    float dist = vec2_length(&vec);

    // if current location is exactly at the center of the circle return edge directly behind vehicle
    if (math_flt_zero(dist)) {
        result->x = circle->_center.x - circle->_radius * circle->_ahrs->cos_yaw;
        result->y = circle->_center.y - circle->_radius * circle->_ahrs->sin_yaw;
        result->z = circle->_center.z;
        return;
    }

    // calculate closest point on edge of circle
    result->x = circle->_center.x + vec.x / dist * circle->_radius;
    result->y = circle->_center.y + vec.y / dist * circle->_radius;
    result->z = circle->_center.z;
}

// init_start_angle - sets the starting angle around the circle and initialises the angle_total
//  if use_heading is true the vehicle's heading will be used to init the angle causing minimum yaw movement
//  if use_heading is false the vehicle's position from the center will be used to initialise the angle
static void circlenav_init_start_angle(mc_circle_t circle, bool use_heading)
{
    // initialise angle total
    circle->_angle_total = 0;

    // if the radius is zero we are doing panorama so init angle to the current heading
    if (circle->_radius <= 0) {
        circle->_angle = circle->_ahrs->yaw;
        return;
    }

    // if use_heading is true
    if (use_heading) {
        circle->_angle = math_wrap_PI(circle->_ahrs->yaw-M_PI);
    } else {
        // if we are exactly at the center of the circle, init angle to directly behind vehicle (so vehicle will backup but not change heading)
        const Vector3f_t curr_pos = ahrs_get_position_neu_cm(circle->_ahrs);
        if (math_flt_equal(curr_pos.x,(float)circle->_center.x) && math_flt_equal(curr_pos.y,(float)circle->_center.y)) {
            circle->_angle = math_wrap_PI(circle->_ahrs->yaw-M_PI);
        } else {
            // get bearing from circle center to vehicle in radians
            float bearing_rad = atan2f(curr_pos.y-(float)circle->_center.y,curr_pos.x-(float)circle->_center.x);
            circle->_angle = math_wrap_PI(bearing_rad);
        }
    }
}

// calc_velocities - calculate angular velocity max and acceleration based on radius and rate
//      this should be called whenever the radius or rate are changed
//      initialises the yaw and current position around the circle
static void circlenav_calc_velocities(mc_circle_t circle, bool init_velocity)
{
    // if we are doing a panorama set the circle_angle to the current heading
    if (circle->_radius <= 0) {
        circle->_angular_vel_max = ToRad(circle->_rate);
        circle->_angular_accel = MAX(fabsf(circle->_angular_vel_max),ToRad(MB_CIRCLE_ANGULAR_ACCEL_MIN));  // reach maximum yaw velocity in 1 second
    }else{
        // calculate max velocity based on waypoint speed ensuring we do not use more than half our max acceleration for accelerating towards the center of the circle
        float velocity_max = MIN(posctrl_get_max_speed_xy_cms(circle->_pos_control), math_sqrtf(0.5f*posctrl_get_max_accel_xy_cmss(circle->_pos_control)*circle->_radius));

        // angular_velocity in radians per second
        circle->_angular_vel_max = velocity_max/circle->_radius;
        circle->_angular_vel_max = math_constrain_float(ToRad(circle->_rate),-circle->_angular_vel_max,circle->_angular_vel_max);

        // angular_velocity in radians per second
        circle->_angular_accel = MAX(posctrl_get_max_accel_xy_cmss(circle->_pos_control)/circle->_radius, ToRad(MB_CIRCLE_ANGULAR_ACCEL_MIN));
    }

    // initialise angular velocity
    if (init_velocity) {
        circle->_angular_vel = 0;
    }
}

// get expected source of terrain data
static enum CircleTerrainSource circlenav_get_terrain_source(mc_circle_t circle)
{
    // use range finder if connected
    if (circle->_rangefinder_available) {
        return CIRCLE_TERRAIN_FROM_RANGEFINDER;
    }
#if defined(AP_TERRAIN_AVAILABLE)
    const AP_Terrain *terrain = AP_Terrain::get_singleton();
    if ((terrain != nullptr) && terrain->enabled()) {
        return CIRCLE_TERRAIN_FROM_TERRAINDATABASE;
    } else {
        return CIRCLE_TERRAIN_UNAVAILABLE;
    }
#else
    return CIRCLE_TERRAIN_UNAVAILABLE;
#endif
}

// get terrain's altitude (in cm above the ekf origin) at the current position (+ve means terrain below vehicle is above ekf origin's altitude)
static bool circlenav_get_terrain_offset(mc_circle_t circle, float* offset_cm)
{
    // calculate offset based on source (rangefinder or terrain database)
    switch (circlenav_get_terrain_source(circle)) {
    case CIRCLE_TERRAIN_UNAVAILABLE:
        return false;
    case CIRCLE_TERRAIN_FROM_RANGEFINDER:
        if (circle->_rangefinder_healthy) {
            *offset_cm = circle->_rangefinder_terrain_offset_cm; // latest rangefinder based terrain offset (e.g. terrain's height above EKF origin);
            return true;
        }
        return false;
    case CIRCLE_TERRAIN_FROM_TERRAINDATABASE:
#if defined(AP_TERRAIN_AVAILABLE)
        float terr_alt = 0.0f;
        AP_Terrain *terrain = AP_Terrain::get_singleton();
        if (terrain != nullptr && terrain->height_above_terrain(terr_alt, true)) {
            *offset_cm = ahrs_get_position_z_up_cm(circle->_ahrs) - (terr_alt * 100.0);
            return true;
        }
#endif
        return false;
    }

    // we should never get here but just in case
    return false;
}

void circlenav_check_param_change(mc_circle_t circle)
{
    if (!math_flt_equal(circle->_last_radius_param,circle->_radius_parm)) {
        circle->_radius = circle->_radius_parm;
        circle->_last_radius_param = circle->_radius_parm;
    }
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static void assign_param(mc_circle_t circle)
{
    param_link_variable(PARAM_ID(CIRCLE, CIRCLE_RADIUS), &circle->_radius_parm);
    param_link_variable(PARAM_ID(CIRCLE, CIRCLE_RATE), &circle->_rate);
    param_link_variable(PARAM_ID(CIRCLE, CIRCLE_OPTIONS), &circle->_options);
}

/*------------------------------------test------------------------------------*/


